﻿#include "camsense_lidar_driver/lidar.h"
#include <cmath>
#include <utility>
#include <vector>
#include <chrono>
#include <functional>
namespace camsense {

int LidarDevice::Initialize(std::function<void(const std::vector<RangeData> &)> callback,
                            const std::string &port,
                            unsigned int baud) {
  if (OpenSerial(port.c_str(), baud) != 0) {
    printf("open serial failed.\n");
    return -1;
  }

  is_scanning_ = true;
  read_thread_ = std::thread(&LidarDevice::LoopReadSerial, this);
  send_thread_ = std::thread(&LidarDevice::LoopSendData, this);
  data_callback_ = std::move(callback);
  return 0;
}

void LidarDevice::LoopReadSerial() {

  while (is_scanning_) {
    auto res = ReadDataSerial();                    // Read signal_ from serial port
//    if (res.second.DN == 0){
//     std::cout<< "res.second.DN " <<res.second.DN<<std::endl;
//      continue;
//    }
//    bool check = CheckData((BYTE *) &res, sizeof(res));
//    if (!check){
//      std::cout<< "CheckData res.second.DN " <<(int)res.second.DN<<std::endl;
//      continue;
//    }
    {
      std::unique_lock<std::mutex> uLock(mutex_);
      array_ector_[write_index % 32] = res;
      write_index++;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }
}
std::vector<RangeData> LidarDevice::CorrectRangeData(std::pair<int64_t, RangeDataInfo> &timed_package) {
//  std::cout <<"CorrectRangeData time = " << timed_package.first<< std::endl;

  auto &package = timed_package.second;
  std::vector<RangeData> res;
  res.reserve(16);
  float angle_delta =
      std::fmod<float, float>(static_cast<float>(static_cast<float>(package.LA - package.FA) / 64.f + 360.f), 360.f)
          / (57.3f * static_cast<float>(package.DN - 1));

  float angle_start = static_cast<float>(package.FA - (0xA000)) / (64 * 57.3f);
  for (size_t i = 0; i < package.DN; i++) {
    auto &pack = package.ranges[i];
    uint16_t is_valid = ((pack.range[1] & 0x80) != 0x80);
    float angle = std::fmod<float, float>(angle_start + angle_delta * (float) i, M_PI * 2.f);
    auto d = (float) (((pack.range[1] << 0) + (pack.range[0] << 8)) & 0x3FFF);
    auto a = center_base_line_ / d;
    auto s = a * a;
    float a_correct = ((-0.0465f * s + 0.16f) * s - 0.3276f) * s * a + a;
    float angle_fix = angle - a_correct;
    float d_fix = std::sqrt(d * d + center_base_line_square_);

    res.emplace_back(RangeData{timed_package.first, is_valid, pack.light, angle, d, d_fix, angle_fix});
  }

  return res;

}

std::pair<int64_t, RangeDataInfo> LidarDevice::ReadDataSerial(unsigned int TimeOut_ms) {
  int rtn{};                                                // Returned value from Read
  bool found = false;
  int find_times = 0;
  BYTE curByte = '\0', preByte = '\0';
  int64_t time;
  while (!found) {
    rtn = serial_.readChar(&curByte, TimeOut_ms);
    if (rtn == 1) {
      found = (preByte == BYTE(0x55)) && (curByte == BYTE(0xaa));
      if (found)
        time = GetTimeStamp();
      preByte = curByte;
    } else {
      printf("k1\n");
      return {};
    }
    if (!found)
      ++find_times;
    else {
      if (find_times > 1)
        std::cout << "uart lost byte " << find_times - 1 << std::endl;
    }
    if (find_times >= 10000000) {
      // Find signal_ header time out!
      printf("k2 %02X\n", curByte);
      return {};
    }
  }

  int read_length = sizeof(RangeDataInfo);
  RangeDataInfo range_data_info;
  rtn = serial_.readChars((unsigned char *) &range_data_info.IF, read_length, TimeOut_ms);
  if (rtn != read_length) {
    printf("k3　%d %d\n", rtn, read_length);
    return {};
  }



//  printf("Data Number  %d ", range_data_info.DN);
//  printf("SP  %f\n", (float) range_data_info.SP / 16.);
//  printf("First Angel  %f, ", (float) (range_data_info.FA - (0xA000)) / 64.);
//  printf("Last Angel  %f, ", (float) (range_data_info.LA - (0xA000)) / 64.);
//  for (auto r:range_data_info.ranges) {
//    std::cout << "range = " << (((r.range[1] << 0) + (r.range[0] << 8)) & 0x3FFF) << ", ";
//    std::cout << "is valid = " << ((r.range[1] & 0x80) != 0x80) << ", ";
//    std::cout << "light = " << (int) r.light << "; \n";
//  }



  return {time, range_data_info};
}

bool LidarDevice::CheckData(const BYTE *buffer, int len) {
  if (len / 2 == 1) {
    printf("it is error.\n");
    return false;
  }

  int *temp = new int[len / 2];//[17] = {0};
  //printf("len:%d\n", len / 2 );
  temp[0] = 0x55 + (0xAA << 8);
  for (int i = 1; i < len / 2; i++) {
    temp[i] = buffer[2 * (i - 1)] + (buffer[2 * (i - 1) + 1] << 8);
  }

  int chk32 = 0;
  for (int i = 0; i < len / 2; i++) {
    chk32 = (chk32 << 1) + temp[i];
  }

  int checksum_target = (chk32 & 0x7FFF) + (chk32 >> 15);
  checksum_target = checksum_target & 0x7FFF;
  int checksum_cur = buffer[len - 2] + (buffer[len - 1] << 8);
  bool is_equal = (checksum_target == checksum_cur);

  delete[] temp;
  return is_equal;
}

int LidarDevice::OpenSerial(const char *port, unsigned int bauds) {
  return serial_.openDevice(port, bauds);
}

void LidarDevice::CloseSerial() {

  serial_.closeDevice();
}

int LidarDevice::Release() {
  is_scanning_ = false;
  read_thread_.join();
  send_thread_.join();

  CloseSerial();

  return 0;
}

int64_t LidarDevice::GetTimeStamp() {
  std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
  std::chrono::nanoseconds ns = now.time_since_epoch();

  return ns.count() / 1000;
}
void LidarDevice::LoopSendData() {
  std::pair<int64_t, RangeDataInfo> data;
  while (is_scanning_) {
    mutex_.lock();
    if (write_index - read_index > 0) {
      data = array_ector_[read_index % 32];
      mutex_.unlock();
      std::vector<RangeData> points = CorrectRangeData(data);
      if (data_callback_) {
        data_callback_(points);
//   　   static auto last_time = points.front().time;
//        std::cout<<" read_index " << read_index<<"  " <<points.front().time<<"   "<<points.front().time - last_time << std::endl;
//        last_time = points.front().time;
        read_index++;
      }
    } else {
      mutex_.unlock();
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

  }
}

}
